/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.tshirtcannon;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Relay;

/**
 * A T-Shirt Cannon. The T-Shirt Cannon is controlled using pneumatics and a
 * finite state machine. The diagrams for the pneumatics and finite state
 * machines are below.
 * <br /><br />
 * Pneumatics diagram:<br />
 * <pre>
 *                           +-----------+
 *                           | Fire Tank |
 *                           +-----------+
 *       +-----------------+       | +--------------+
 *       | Pressure Switch |       +-| Fire Valve 1 | +-------+
 *       +-----------------+       | +--------------+ | Light |
 * +------------+ | +------------+ | +--------------+ +-------+
 * | Compressor |-+-| Fill Valve |-+-| Fire Valve 2 |
 * +------------+ | +------------+ | +--------------+
 *          +-----------+   +------------+
 *          | Main Tank |   | Dump Valve |
 *          +-----------+   +------------+
 * </pre>
 *
 * State diagram:<br />
 * <pre>
 * +--------------------+----------------------+--------------------------+
 * | State Name: Safety | State Name: Disabled | State Name: Pressurizing |
 * | Compressor: Stop   | Compressor: Stop     | Compressor: Start        |
 * | Fill Valve: On     | Fill Valve: Off      | Fill Valve: Off          |
 * | Dump Valve: On     | Dump Valve: On       | Dump Valve: On           |
 * | Fire Valve 1: Off  | Fire Valve 1: Off    | Fire Valve 1: Off        |
 * | Fire Valve 2: Off  | Fire Valve 2: Off    | Fire Valve 2: Off        |
 * | Light: Off         | Light: Off           | Light: Off               |
 * +--------------------+----+-----------------+-----+--------------------+-+
 * | State Name: Pressurized | State Name: Filling 1 | State Name: Filled 1 |
 * | Compressor: Start       | Compressor: Start     | Compressor: Start    |
 * | Fill Valve: Off         | Dump Valve: Off       | Fill Valve: Off      |
 * | Dump Valve: On          | Wait                  | Dump Valve: Off      |
 * | Fire Valve 1: Off       | Fill Valve: On        | Fill Valve 1: Off    |
 * | Fire Valve 2: Off       | Fire Valve 1: Off     | Fill Valve 2: Off    |
 * | Light: Off              | Fire Valve 2: Off     | Light: On            |
 * |                         | Light: On             |                      |
 * +----------------------+--+--------------------+--+-------------------+--+
 * | State Name: Firing 1 | State Name: Filling 2 | State Name: Filled 2 |
 * | Compressor: Start    | Compressor: Start     | Compressor: Start    |
 * | Fill Valve: Off      | Dump Valve: Off       | Fill Valve: Off      |
 * | Dump Valve: Off      | Fill Valve: On        | Dump Valve: Off      |
 * | Fire Valve 1: On     | Fire Valve 1: Off     | Fill Valve 1: Off    |
 * | Fire Valve 2: Off    | Fire Valve 2: Off     | Fill Valve 2: Off    |
 * | Light: On            | Light: On             | Light: On            |
 * +----------------------+-----------------------+----------------------+
 * | State Name: Firing 1 |
 * | Compressor: Start    |
 * | Fill Valve: Off      |
 * | Dump Valve: Off      |
 * | Fire Valve 1: Off    |
 * | Fire Valve 2: On     |
 * | Light: On            |
 * +----------------------+
 * </pre>
 *
 * The state transition diagram is in "State Transition Diagram.doc".
 * </pre>
 */
public class TShirtCannon {

    private TShirtCannonState m_state;  // The current state.
    private Compressor m_compressor;    // The compressor.
    private Solenoid m_fillValve;       // The fill valve.
    private Solenoid m_dumpValve;       // The dump valve.
    private Solenoid m_fireValve1;      // The first fire valve.
    private Solenoid m_fireValve2;      // The second fire valve.
    private Joystick m_leftStick;       // The left joystick.
    private Joystick m_rightStick;      // The right joystick.
    private Joystick m_gigaware;        // The gigaware controller.
    private Timer m_timer;              // The timer.
    private Relay m_light;              // Relay for the light.
    private boolean enabled1 = true;
    private boolean enabled2 = true;
    private boolean fired1 = false;
    private boolean fired2 = false;
    private boolean debounce1 = false;
    private boolean debounce2 = false;

    /**
     * The constructor for a TShirtCannon.
     *
     * @param compressor The compressor.
     * @param fillValve The fill valve.
     * @param dumpValve The dump valve.
     * @param fireValve1 The first fire valve.
     * @param fireValve2 The second fire valve.
     * @param light The relay for the saftey light.
     * @param leftStick The left joystick.
     * @param rightStick The right joystick.
     * @param gigaware The gigaware controller.
     * @see TShirtCannon
     */
    public TShirtCannon(Compressor compressor, Solenoid fillValve,
            Solenoid dumpValve, Solenoid fireValve1,
            Solenoid fireValve2, Relay light, Joystick leftStick,
            Joystick rightStick, Joystick gigaware) {
        // Set all the varibles to the parameters.
        m_compressor = compressor;
        m_fillValve = fillValve;
        m_dumpValve = dumpValve;
        m_fireValve1 = fireValve1;
        m_fireValve2 = fireValve2;
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_gigaware = gigaware;
        m_light = light;

        // Initalize the timer.
        m_timer = new Timer();
        m_timer.reset();

        // Set the current state to be disabled.
        m_state = DisabledState.instance();

        // Enter the current state.
        m_state.enter(this);
    }

    /**
     * Gets the current state's name.
     *
     * @return The current state name.
     * @see TShirtCannon
     */
    public String getCurrentStateName() {
        return m_state.getName();
    }

    /**
     * Updates the current state.
     * @see TShirtCannon
     */
    public void update() {
        m_state.update(this);
    }

    /**
     * Changes to a new state.
     * @param s The state to change to.
     * @see TShirtCannon
     */
    void changeState(TShirtCannonState s) {
        m_state.exit(this);
        m_state = s;
        s.enter(this);
    }

    /**
     * Gets the compressor's state.
     * @return The compressor's state (on / off).
     * @see TShirtCannon
     */
    public boolean getCompressorState() {
        return m_compressor.enabled();
    }

    /**
     * Sets the compressor's state.
     * @param bState The compressor's new state (on / off).
     * @see TShirtCannon
     */
    public void setCompressorState(boolean bState) {
        if (bState) {
            m_compressor.start();
        } else {
            m_compressor.stop();
        }
    }

    /**
     * Get the pressure switch state.
     * @return The current state of the pressure switch (on / off).
     * @see TShirtCannon
     */
    public boolean getPressureSwitchState() {
        return m_compressor.getPressureSwitchValue();
    }

    /**
     * Gets the state of the fill valve.
     * @return The current state of the fill valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public boolean getFillValveState() {
        return m_fillValve.get();
    }

    /**
     * Sets the state of the fill valve.
     * @param on The new state of the fill valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public void setFillValveState(boolean on) {
        m_fillValve.set(on);
    }

    /**
     * Gets the state of the dump valve.
     * @return The current state of the dump valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public boolean getDumpValveState() {
        return m_dumpValve.get();
    }

    /**
     * Sets the state of the dump valve.
     * @param on The new state of the dump valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public void setDumpValveState(boolean on) {
        m_dumpValve.set(on);
    }

    /**
     * Gets the state of the first fire valve.
     * @return The current state of the first fire valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public boolean getFireValve1State() {
        return m_fireValve1.get();
    }

    /**
     * Sets the state of the first fire valve.
     * @param on The new state of the first fire valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public void setFireValve1State(boolean on) {
        m_fireValve1.set(on);
    }

    /**
     * Gets the state of the second fire valve.
     * @return The current state of the second fire valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public boolean getFireValve2State() {
        return m_fireValve2.get();
    }

    /**
     * Sets the state of the second fire valve.
     * @param on The new state of the second fire valve (true is open. false is closed).
     * @see TShirtCannon
     */
    public void setFireValve2State(boolean on) {
        m_fireValve2.set(on);
    }

    /**
     * Gets the left joystick.
     * @return The left joystick.
     * @see TShirtCannon
     */
    public Joystick getLeftStick() {
        return m_leftStick;
    }

    /**
     * Gets the right joystick.
     * @return The right joystick
     * @see TShirtCannon
     */
    public Joystick getRightStick() {
        return m_rightStick;
    }

    /**
     * Gets the gigaware controller.
     * @return The gigaware controller.
     * @see TShirtCannon
     */
    public Joystick getGigaware() {
        return m_gigaware;
    }

    /**
     * Gets the state of the light.
     * @return The light's state (on / off).
     * @see TShirtCannon
     */
    public boolean getLightState() {
        //TODO: Find out what the electrical team is using for the light
        return false;
    }

    /**
     * Sets the state of the light
     * @param on The new state of the light (on / off).
     * @see TShirtCannon
     */
    void setLightState(boolean on) {
        if (on) {
            m_light.set(Relay.Value.kForward);
        } else {
            m_light.set(Relay.Value.kOff);
        }
    }

    /**
     * Changes to the disabled state.
     * @see TShirtCannon
     */
    public void disable() {
        changeState(DisabledState.instance());
    }

    /**
     * Starts the timer.
     * @see TShirtCannon
     */
    void startTimer() {
        m_timer.start();
        m_timer.reset();
    }

    /**
     * Gets the number of seconds that the timer was running. The timer is reset
     * to zero when something calls startTimer or stopTimer.
     * @return The number of second that the timer was running.
     * @see startTimer()
     * @see stopTimer()
     * @see TShirtCannon
     */
    public double getTimerSeconds() {
        return m_timer.get();
    }

    /**
     * Stops the timer.
     * @see TShirtCannon
     */
    void stopTimer() {
        m_timer.stop();
        m_timer.reset();
    }

    public boolean getEnabled(int i) {
        if (i == 1) {
            return enabled1;
        } else if (i == 2) {
            return enabled2;
        } else {
            throw new ArrayIndexOutOfBoundsException();
        }
    }

    public boolean getFired(int i) {
        if (i == 1) {
            return fired1;
        } else if (i == 2) {
            return fired2;
        } else {
            throw new ArrayIndexOutOfBoundsException();
        }
    }

    public boolean getDebounce(int i) {
        if (i == 1) {
            return debounce1;
        } else if (i == 2) {
            return debounce2;
        } else {
            throw new ArrayIndexOutOfBoundsException();
        }
    }

    boolean isEmpty() {
        return ((getFired(1) || !getEnabled(1))
                && (getFired(2) || !getEnabled(2)));
    }

    void setFired(int i, boolean isFired) {
        if (i == 1) {
            fired1 = isFired;
        } else if(i == 2) {
            fired2 = isFired;
        } else {
            throw new ArrayIndexOutOfBoundsException();
        }
    }

    void setEnabled(int i, boolean isEnabled) {
        if (i == 1) {
            enabled1 = isEnabled;
        } else if(i == 2) {
            enabled2 = isEnabled;
        } else {
            throw new ArrayIndexOutOfBoundsException();
        }
    }

    void setDebounce(int i, boolean isEnabled) {
        if (i == 1) {
            debounce1 = isEnabled;
        } else if(i == 2) {
            debounce2 = isEnabled;
        } else {
            throw new ArrayIndexOutOfBoundsException();
        }
    }
}
